#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import rospy
import actionlib
import os
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from math import pi
from std_msgs.msg import String
import random
random_distance=2

#初始化节点
rospy.init_node('random_nav',anonymous=False)

#设置参数 
square_size = rospy.get_param('~square_size',1.0)
#action服务器连接
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo('等待move_base action服务器连接...')
move_base.wait_for_server(rospy.Duration(30))
rospy.loginfo('已连接导航服务')

#添加坐标点,输入x（前）坐标，y（左）坐标，th（平面朝向0～360度）
def nav_to(x,y,d):
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id='map'
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose = pose_e(x,y,d)
    move(goal)

#写一个函数 用于任务完成提示。
def move(goal):
    move_base.send_goal(goal)
    finished_within_time = move_base.wait_for_result(rospy.Duration(10))
    if not finished_within_time:
        move_base.cancel_goal()
        rospy.loginfo('时间超时，导航任务取消。')
    state = move_base.get_state()
    if state == GoalStatus.SUCCEEDED:
		rospy.loginfo('导航成功！')
    
def shutdown():
    rospy.loginfo('机器人任务停止')
    move_base.cancel_goal()
    rospy.sleep(2)
    cmd_vel_pub.publish(Twist)
    rospy.sleep(1)
    
def pose_e(x,y,th):#输入x（前）坐标，y（左）坐标，th（平面朝向0～360度）
    new_pose=Pose()
    new_pose.position.x=float(x)
    new_pose.position.y=float(y)
    #机器朝向，平面朝向弧度转化成四元数空间位姿
    q=quaternion_from_euler(0.0,0.0,float(th)/180.0*pi)
    new_pose.orientation.x=q[0]
    new_pose.orientation.y=q[1]
    new_pose.orientation.z=q[2]
    new_pose.orientation.w=q[3]
    return  new_pose
			
if __name__ == '__main__':
    while True:
        x=random.uniform(-random_distance,random_distance)
        y=random.uniform(-random_distance,random_distance)
        t=random.randint(-360,360)
        nav_to(x,y,t)



       

        







